#include <memory>
#include <chrono>
#include <vector>
#include <algorithm>
#include <cmath>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "fruit_nav/action/navigate_to_pose.hpp"
#include "arm_control/action/arm.hpp"
#include "qr_recognition/action/recognize_qr.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "std_msgs/msg/int32.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

using namespace std::chrono_literals;

class TaskPlannerC : public rclcpp::Node
{
public:
  using NavigateToPose = fruit_nav::action::NavigateToPose;
  using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;
  using ARM = arm_control::action::ARM;
  using GoalHandleARM = rclcpp_action::ClientGoalHandle<ARM>;
  using RecognizeQR = qr_recognition::action::RecognizeQR;
  using GoalHandleRecognizeQR = rclcpp_action::ClientGoalHandle<RecognizeQR>;

  // 定义目标点结构体
  struct WayPoint
  {
    double x;
    double y;
    double yaw;  // 弧度
  };

  TaskPlannerC() : Node("task_planner_node_c"), current_goal_index_(0)
  {
    RCLCPP_INFO(this->get_logger(), "C区任务规划节点已启动");

    // 创建action客户端
    this->action_client_ = rclcpp_action::create_client<NavigateToPose>(
      this, "/navigate_to_pose");
    
    // 创建ARM action客户端
    this->arm_action_client_ = rclcpp_action::create_client<ARM>(
      this, "/ARM");
    
    // 创建RecognizeQR action客户端
    this->qr_action_client_ = rclcpp_action::create_client<RecognizeQR>(
      this, "/recognize_qr");

    // 创建detect_part发布器
    this->detect_part_publisher_ = this->create_publisher<std_msgs::msg::Int32>(
      "/detect_part", 10);

    // 启动时发布3到/detect_part（C区检测）
    auto msg = std_msgs::msg::Int32();
    msg.data = 3;
    detect_part_publisher_->publish(msg);
    RCLCPP_INFO(this->get_logger(), "已发布detect_part = 3（C区启动）");

    // 定义目标点11（C区入口）
    waypoints_ = {
      {0.1, -2.66, -1.57}  // 目标点11: x=0.1m, y=-2.66m, yaw=-1.57rad C区入口
    };

    RCLCPP_INFO(this->get_logger(), "已定义 %zu 个初始目标点", waypoints_.size());

    // 等待action服务器可用
    RCLCPP_INFO(this->get_logger(), "等待action服务器 /navigate_to_pose ...");
    if (!action_client_->wait_for_action_server(10s))
    {
      RCLCPP_ERROR(this->get_logger(), "Action服务器不可用，请确保fruit_nav节点已启动");
      rclcpp::shutdown();
      return;
    }

    RCLCPP_INFO(this->get_logger(), "Action服务器已连接");

    // 开始发送第一个目标
    send_next_goal();
  }

private:
  rclcpp_action::Client<NavigateToPose>::SharedPtr action_client_;
  rclcpp_action::Client<ARM>::SharedPtr arm_action_client_;
  rclcpp_action::Client<RecognizeQR>::SharedPtr qr_action_client_;
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr detect_part_publisher_;
  std::vector<WayPoint> waypoints_;
  size_t current_goal_index_;
  
  // 存储QR识别结果（type2 - C区）
  std::vector<int32_t> qr_type2_fruit_array_;
  std::vector<int32_t> qr_type2_number_array_;

  // 将yaw角转换为四元数
  geometry_msgs::msg::Quaternion yaw_to_quaternion(double yaw)
  {
    tf2::Quaternion q;
    q.setRPY(0, 0, yaw);
    return tf2::toMsg(q);
  }

  // 根据数字（1-12）获取对应的位置和姿态
  // 布局（2行）：
  //   D                    C
  // 4   3   2   1     （顶部y=-1.9）
  // 8   7   6   5/9-12（底部y=-2.67）
  //   A                    B
  // 循环路径: A→B→C→D→A（逆时针）
  WayPoint get_position_for_number(int32_t number)
  {
    WayPoint point;
    
    switch(number)
    {
      // 顶部行（y=-1.9）：数字1-4，朝向左侧（yaw=3.14）
      case 1:
        point = {2.1, -1.9, 3.14};
        break;
      case 2:
        point = {1.61, -1.9, 3.14};
        break;
      case 3:
        point = {1.2, -1.9, 3.14};
        break;
      case 4:
        point = {0.61, -1.9, 3.14};
        break;
      
      // 底部行（y=-2.67）：数字5-8，朝向右侧（yaw=0.0）
      case 5:
        point = {1.85, -2.67, 0.0};
        break;
      case 6:
        point = {1.37, -2.67, 0.0};
        break;
      case 7:
        point = {0.86, -2.67, 0.0};
        break;
      case 8:
        point = {0.36, -2.67, 0.0};
        break;
      
      // 底部行（y=-2.67）：数字9-12，位置与5-8相同，朝向右侧（yaw=0.0）
      case 9:
        point = {1.85, -2.67, 0.0};  // 与5相同
        break;
      case 10:
        point = {1.37, -2.67, 0.0};  // 与6相同
        break;
      case 11:
        point = {0.86, -2.67, 0.0};  // 与7相同
        break;
      case 12:
        point = {0.36, -2.67, 0.0};  // 与8相同
        break;
      
      default:
        RCLCPP_WARN(this->get_logger(), "未知的数字: %d，使用默认位置", number);
        point = {0.1, -2.66, -1.57};
        break;
    }
    
    return point;
  }

  // 判断数字点是否在路径段上
  // segment: 0=A→AB中点, 1=AB中点→B, 2=B→C, 3=C→CD中点, 4=CD中点→D, 5=D→A
  // 仅在段0,1(底部)和段3,4(顶部)添加数字目标
  bool is_number_on_segment(int32_t num, int segment)
  {
    WayPoint point = get_position_for_number(num);
    
    const double top_y = -1.9;     // 顶部y坐标
    const double bottom_y = -2.67; // 底部y坐标
    
    switch(segment)
    {
      case 0: // A→AB中点：底部左半段（数字8,12,11,7）
        return (std::abs(point.y - bottom_y) < 0.1);  // y ≈ -2.67
        
      case 1: // AB中点→B：底部右半段（数字6,10,5,9）
        return (std::abs(point.y - bottom_y) < 0.1);  // y ≈ -2.67
        
      case 2: // B→C：右侧过渡段，不添加数字目标
        return false;
        
      case 3: // C→CD中点：顶部右半段（数字1,2）
        return (std::abs(point.y - top_y) < 0.1);  // y ≈ -1.9
        
      case 4: // CD中点→D：顶部左半段（数字3,4）
        return (std::abs(point.y - top_y) < 0.1);  // y ≈ -1.9
        
      case 5: // D→A：左侧过渡段，不添加数字目标
        return false;
        
      default:
        return false;
    }
  }
  
  // 在路径段上收集所有需要访问的数字点，并按路径方向排序
  std::vector<int32_t> get_numbers_on_segment(int segment, const std::vector<int32_t>& remaining_numbers)
  {
    std::vector<std::pair<double, int32_t>> numbers_with_position;
    
    for (int32_t num : remaining_numbers)
    {
      if (is_number_on_segment(num, segment))
      {
        WayPoint point = get_position_for_number(num);
        double sort_key = 0.0;
        
        switch(segment)
        {
          case 0: sort_key = point.x; break;   // A→AB中点（底部左）：x递增（从左到右）
          case 1: sort_key = point.x; break;   // AB中点→B（底部右）：x递增（从左到右）
          case 2: sort_key = -point.y; break;  // B→C（右侧）：y递增（从下到上）
          case 3: sort_key = -point.x; break;  // C→CD中点（顶部右）：x递减（从右到左）
          case 4: sort_key = -point.x; break;  // CD中点→D（顶部左）：x递减（从右到左）
          case 5: sort_key = point.y; break;   // D→A（左侧）：y递减（从上到下）
        }
        
        numbers_with_position.push_back({sort_key, num});
      }
    }
    
    // 按排序键排序
    std::sort(numbers_with_position.begin(), numbers_with_position.end());
    
    std::vector<int32_t> result;
    for (const auto& pair : numbers_with_position)
    {
      result.push_back(pair.second);
    }
    
    return result;
  }

  // 根据数字数组生成按路径规划的目标点序列（循环模式）
  void generate_waypoints_from_numbers(const std::vector<int32_t>& numbers)
  {
    RCLCPP_INFO(this->get_logger(), "======================================");

    RCLCPP_INFO(this->get_logger(), "根据数字数组生成目标点序列（A→B→C→D循环模式 - 优化版）...");
    
    // 打印接收到的数字数组
    std::string numbers_str = "数字数组: [";
    for (size_t i = 0; i < numbers.size(); ++i)
    {
      numbers_str += std::to_string(numbers[i]);
      if (i < numbers.size() - 1)
        numbers_str += ", ";
    }
    numbers_str += "]";
    RCLCPP_INFO(this->get_logger(), "%s", numbers_str.c_str());
    
    // 清空当前的目标点列表，保留前1个已完成的目标点（目标点11）
    waypoints_.resize(1);
    
    // 定义循环路径的6个过渡点 A→AB中点→B→C→CD中点→D→A（逆时针）
    WayPoint corner_points[6] = {
      {0.1, -2.67, 0.0},      // 0: A（左下角，朝右）
      {1.23, -2.67, 0.0},     // 1: AB中点（底部中点，朝右）
      {2.36, -2.67, 1.57},    // 2: B（右下角，朝上）
      {2.36, -1.9, 3.14},     // 3: C（右上角，朝左）
      {1.23, -1.9, 3.14},     // 4: CD中点（顶部中点，朝左）
      {0.1, -1.9, -1.57}      // 5: D（左上角，朝下）
    };
    
    // 路径段名称（用于日志）
    const char* segment_names[6] = {
      "A→AB中点(底部左)", "AB中点→B(底部右)", "B→C(右侧)", 
      "C→CD中点(顶部右)", "CD中点→D(顶部左)", "D→A(左侧)"
    };
    
    size_t next_target_index = 0;  // 下一个要访问的数字在原数组中的索引
    
    RCLCPP_INFO(this->get_logger(), "开始循环路径，一次循环可访问多个数字点...");
    
    // 循环直到所有数字都被访问
    int max_loops = 100;  // 防止无限循环
    int loop_count = 0;
    
    while (next_target_index < numbers.size() && loop_count < max_loops)
    {
      loop_count++;
      RCLCPP_INFO(this->get_logger(), "--- 循环 #%d ---", loop_count);
      
      // 遍历6条路径段
      for (int segment = 0; segment < 6; segment++)
      {
        // 收集当前路径段上的所有连续数字目标
        std::vector<int32_t> targets_on_segment;
        size_t temp_index = next_target_index;
        
        // 从next_target_index开始，收集所有在当前路径段上的连续数字
        while (temp_index < numbers.size() && is_number_on_segment(numbers[temp_index], segment))
        {
          targets_on_segment.push_back(numbers[temp_index]);
          temp_index++;
        }
        
        if (!targets_on_segment.empty())
        {
          // 有目标在此段，添加过渡点
          const char* point_names[6] = {"A", "AB中点", "B", "C", "CD中点", "D"};
          waypoints_.push_back(corner_points[segment]);
          RCLCPP_INFO(this->get_logger(), "  过渡点%s: (%.2f, %.2f, yaw=%.2f)", 
                      point_names[segment], corner_points[segment].x, corner_points[segment].y, corner_points[segment].yaw);
          
          // 按路径方向对这些目标排序
          std::vector<std::pair<double, int32_t>> sorted_targets;
          for (int32_t num : targets_on_segment)
          {
            WayPoint point = get_position_for_number(num);
            double sort_key = 0.0;
            
            switch(segment)
            {
              case 0: sort_key = point.x; break;   // A→AB中点：x递增
              case 1: sort_key = point.x; break;   // AB中点→B：x递增
              case 2: sort_key = -point.y; break;  // B→C：y递增
              case 3: sort_key = -point.x; break;  // C→CD中点：x递减
              case 4: sort_key = -point.x; break;  // CD中点→D：x递减
              case 5: sort_key = point.y; break;   // D→A：y递减
            }
            
            sorted_targets.push_back({sort_key, num});
          }
          
          // 按排序键排序
          std::sort(sorted_targets.begin(), sorted_targets.end());
          
          // 依次添加所有排序后的目标点
          for (const auto& pair : sorted_targets)
          {
            int32_t num = pair.second;
            WayPoint target_point = get_position_for_number(num);
            waypoints_.push_back(target_point);
            RCLCPP_INFO(this->get_logger(), "  ✓ 访问数字%d: (%.2f, %.2f, yaw=%.2f) [%s]", 
                        num, target_point.x, target_point.y, target_point.yaw, segment_names[segment]);
          }
          
          // 更新next_target_index
          next_target_index = temp_index;
        }
        else
        {
          // 没有目标在此段，只添加过渡点
          const char* point_names[6] = {"A", "AB中点", "B", "C", "CD中点", "D"};
          waypoints_.push_back(corner_points[segment]);
          RCLCPP_INFO(this->get_logger(), "  过渡点%s: (%.2f, %.2f, yaw=%.2f)", 
                      point_names[segment], corner_points[segment].x, corner_points[segment].y, corner_points[segment].yaw);
        }
      }
      
      // 如果所有数字都已访问，退出循环
      if (next_target_index >= numbers.size())
      {
        RCLCPP_INFO(this->get_logger(), "所有数字点已访问完成！");
        break;
      }
    }
    
    // 添加最终目标点（返回到A点）
    RCLCPP_INFO(this->get_logger(), "添加最终目标点A: (0.1, -2.67, 0.0)");
    waypoints_.push_back({0.1, -2.67, 0.0});
    
    RCLCPP_INFO(this->get_logger(), "总共生成 %zu 个目标点", waypoints_.size() - 1);
    RCLCPP_INFO(this->get_logger(), "======================================");
  }

  // 发送下一个目标点
  void send_next_goal()
  {
    if (current_goal_index_ >= waypoints_.size())
    {
      RCLCPP_INFO(this->get_logger(), "========================================");
      RCLCPP_INFO(this->get_logger(), "所有目标点已完成！任务规划执行结束。");
      RCLCPP_INFO(this->get_logger(), "========================================");
      return;
    }

    auto& waypoint = waypoints_[current_goal_index_];

    RCLCPP_INFO(this->get_logger(), ">>> 目标点 [%zu/%zu]: (%.2f, %.2f, yaw=%.2f)", 
                current_goal_index_ + 1, waypoints_.size(),
                waypoint.x, waypoint.y, waypoint.yaw);

    // 创建目标消息
    auto goal_msg = NavigateToPose::Goal();
    goal_msg.target_pose.header.frame_id = "map";
    goal_msg.target_pose.header.stamp = this->now();
    goal_msg.target_pose.pose.position.x = waypoint.x;
    goal_msg.target_pose.pose.position.y = waypoint.y;
    goal_msg.target_pose.pose.position.z = 0.0;
    goal_msg.target_pose.pose.orientation = yaw_to_quaternion(waypoint.yaw);

    // 配置发送选项
    auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
    
    // 设置目标响应回调
    send_goal_options.goal_response_callback =
      std::bind(&TaskPlannerC::goal_response_callback, this, std::placeholders::_1);
    
    // 设置反馈回调
    send_goal_options.feedback_callback =
      std::bind(&TaskPlannerC::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
    
    // 设置结果回调
    send_goal_options.result_callback =
      std::bind(&TaskPlannerC::result_callback, this, std::placeholders::_1);

    // 发送目标
    action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 目标响应回调
  void goal_response_callback(const GoalHandleNavigateToPose::SharedPtr & goal_handle)
  {
    if (!goal_handle)
    {
      RCLCPP_ERROR(this->get_logger(), "目标被服务器拒绝");
      current_goal_index_++;
      send_next_goal();
    }
    // 目标被接受时不输出日志，减少冗余信息
  }

  // 反馈回调
  void feedback_callback(
    GoalHandleNavigateToPose::SharedPtr,
    const std::shared_ptr<const NavigateToPose::Feedback> feedback)
  {
    // 减少日志输出，仅在需要调试时取消注释
    // RCLCPP_INFO(this->get_logger(), 
    //             "当前位置: x=%.2f, y=%.2f | 剩余距离: %.2f m", 
    //             feedback->current_pose.pose.position.x,
    //             feedback->current_pose.pose.position.y,
    //             feedback->distance_remaining);
  }

  // 结果回调
  void result_callback(const GoalHandleNavigateToPose::WrappedResult & result)
  {
    switch (result.code)
    {
      case rclcpp_action::ResultCode::SUCCEEDED:
        RCLCPP_INFO(this->get_logger(), "✓ 目标点 %zu 完成: %s", 
                    current_goal_index_ + 1, 
                    result.result->message.c_str());
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "✗ 目标点 %zu 中止: %s", 
                     current_goal_index_ + 1,
                     result.result->message.c_str());
        break;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_WARN(this->get_logger(), "✗ 目标点 %zu 被取消", 
                    current_goal_index_ + 1);
        break;
      default:
        RCLCPP_ERROR(this->get_logger(), "✗ 目标点 %zu 未知结果", 
                     current_goal_index_ + 1);
        break;
    }

    // 如果是目标点11（索引0，C区入口），并且成功到达，则执行扫码动作2
    if (result.code == rclcpp_action::ResultCode::SUCCEEDED && 
        current_goal_index_ == 0)
    {
      RCLCPP_INFO(this->get_logger(), "到达目标点 %zu（C区入口），准备执行扫码动作2...", 
                  current_goal_index_ + 1);
      
      std::this_thread::sleep_for(2s);
      execute_qr_recognition_type2();
    }
    else
    {
      // 继续发送下一个目标点
      current_goal_index_++;
      
      // 等待一小段时间后发送下一个目标
      if (current_goal_index_ < waypoints_.size())
      {
        std::this_thread::sleep_for(2s);
        send_next_goal();
      }
      else
      {
        send_next_goal();  // 打印完成信息
      }
    }
  }

  // 执行二维码识别type2（C区）
  void execute_qr_recognition_type2()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行二维码识别（type2 - C区）...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待RecognizeQR action服务器可用
    if (!qr_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "RecognizeQR Action服务器不可用，跳过二维码识别");
      // 继续下一个目标点
      current_goal_index_++;
      send_next_goal();
      return;
    }

    auto goal_msg = RecognizeQR::Goal();
    goal_msg.type = 2;  // 使用type2进行识别
    
    RCLCPP_INFO(this->get_logger(), "发送二维码识别请求（type=2）...");

    auto send_goal_options = rclcpp_action::Client<RecognizeQR>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleRecognizeQR::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 二维码识别（type2）完成");
          
          // 记录返回的数据
          qr_type2_fruit_array_ = result.result->fruit_array;
          qr_type2_number_array_ = result.result->number_array;
          
          RCLCPP_INFO(this->get_logger(), "======================================");
          RCLCPP_INFO(this->get_logger(), "二维码识别结果（type2 - C区）：");
          
          // 打印fruit_array
          if (!qr_type2_fruit_array_.empty())
          {
            std::string fruit_str = "  fruit_array: [";
            for (size_t i = 0; i < qr_type2_fruit_array_.size(); ++i)
            {
              fruit_str += std::to_string(qr_type2_fruit_array_[i]);
              if (i < qr_type2_fruit_array_.size() - 1)
                fruit_str += ", ";
            }
            fruit_str += "]";
            RCLCPP_INFO(this->get_logger(), "%s", fruit_str.c_str());
          }
          else
          {
            RCLCPP_INFO(this->get_logger(), "  fruit_array: []");
          }
          
          // 打印number_array
          if (!qr_type2_number_array_.empty())
          {
            std::string number_str = "  number_array: [";
            for (size_t i = 0; i < qr_type2_number_array_.size(); ++i)
            {
              number_str += std::to_string(qr_type2_number_array_[i]);
              if (i < qr_type2_number_array_.size() - 1)
                number_str += ", ";
            }
            number_str += "]";
            RCLCPP_INFO(this->get_logger(), "%s", number_str.c_str());
          }
          else
          {
            RCLCPP_INFO(this->get_logger(), "  number_array: []");
          }
          
          RCLCPP_INFO(this->get_logger(), "======================================");
          
          // 根据数字数组生成新的目标点序列
          if (!qr_type2_number_array_.empty())
          {
            RCLCPP_INFO(this->get_logger(), "根据识别结果生成C区导航路径...");
            generate_waypoints_from_numbers(qr_type2_number_array_);
            
            // 继续执行新生成的目标点
            current_goal_index_++;
            std::this_thread::sleep_for(2s);
            send_next_goal();
          }
          else
          {
            RCLCPP_WARN(this->get_logger(), "数字数组为空，跳过C区导航");
            RCLCPP_INFO(this->get_logger(), "所有任务已完成！");
          }
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 二维码识别（type2）失败");
          // 失败则不生成新的目标点，任务结束
          RCLCPP_INFO(this->get_logger(), "二维码识别失败，任务结束");
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleRecognizeQR::SharedPtr,
             const std::shared_ptr<const RecognizeQR::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "识别状态: %s", feedback->status.c_str());
      };

    qr_action_client_->async_send_goal(goal_msg, send_goal_options);
  }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<TaskPlannerC>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

